#include "zf_common_headfile.h"


static float_data fdata[CH_COUNT + 1];
static uint8 Send_En = 0;
static uint8 flag = 1;


void Frame_init(void)
{
    unsigned int x = 1;
    unsigned char *endian_check = (unsigned char*)&x;

    if(endian_check[0] != 1)
    {
        flag = 0;
    }
    unsigned char tail[4] = {0x00, 0x00, 0x80, 0x7f};

    memcpy(fdata[CH_COUNT].send, tail, sizeof(tail));
}




static inline void Chage_BS(unsigned char*data)
{
    if(flag)
    {
        return;
    }

    for(int i = 0; i < 2; i++)
    {
        unsigned char temp = data[i];
        data[i] = data[3 - i];
        data[3 - i] = temp;
    }
}




void Data_Init(int CH, float data)
{
    fdata[CH].data = data;
    Chage_BS(fdata[CH].send);
}



void Send_Data(void)
{
    unsigned char *p = fdata[0].send;
    if(Send_En)
    {
        JF_Send(p, 4 * (CH_COUNT + 1));
    }
}



void Send_Data_Num(int num)
{
    unsigned char tail[4] = {0x00, 0x00, 0x80, 0x7f};

    memcpy(fdata[num].send, tail, sizeof(tail));

    unsigned char *p = fdata[0].send;

    if(Send_En)
    {
        JF_Send(p, 4 * (num + 1));
    }
}





void Data_Init_All(const int num,...)
{
    va_list Valist;
    int i = 0;
    va_start(Valist, num);

    for(i = 0; i < num; ++i)
    {
        fdata[i].data = va_arg(Valist, MY_Float);
        Chage_BS(fdata[i].send);
    }

    va_end(Valist);
}





void MyPrintf(const char *arg,...)
{
   uint8 send[100] = {0};

   va_list Valist;
   va_start(Valist, arg);

   vsprintf((char*)send, arg, Valist);

   va_end(Valist);

   if(Send_En)
   {
       JF_Send(send, strlen((char*)send));
   }
}





void Send_Enable(void)
{
    Send_En = 1;
}




void Send_Disable(void)
{
    Send_En = 0;
}




//获取数据，支持0.01-999.99的数据范围
//帧格式P1=2.45!  I1=22.45!  D1=287.45!
float Get_Gata(uint8* data,uint32 len, uint8 mode)
{
    float num;
    if(mode==1)//数据完整
    {
        if(len==8){num = (data[3]-'0')*1 + (data[5]-'0')*0.1+(data[6]-'0')*0.01;}
        if(len==9){num = (data[3]-'0')*10 + (data[4]-'0')*1+(data[6]-'0')*0.1+(data[7]-'0')*0.01;}
        if(len==10){num = (data[3]-'0')*100 + (data[4]-'0')*10 + (data[5]-'0')*1 +(data[7]-'0')*0.1+(data[8]-'0')*0.01;}
        if(len==7){num = (data[2]-'0')*1000 + (data[3]-'0')*100 + (data[4]-'0')*10 + (data[5]-'0')*1;}
        if(len==6){num = (data[2]-'0')*100 + (data[3]-'0')*10 + (data[4]-'0')*1;}
    }
    if(mode==2)//数据不完整
    {
        for(int i=0;i<len;i++)
        {
            if((data[i] == 'P')&&(data[i+1] == '1')&&(data[i+2]=='='))
            {
                if(data[i+7]=='!'){num = (data[i+3]-'0')*1 + (data[i+5]-'0')*0.1+(data[i+6]-'0')*0.01;}
                if(data[i+8]=='!'){num = (data[i+3]-'0')*10 + (data[i+4]-'0')*1+(data[i+6]-'0')*0.1+(data[i+7]-'0')*0.01;}
                if(data[i+9]=='!'){num = (data[i+3]-'0')*100 + (data[i+4]-'0')*10 + (data[i+5]-'0')*1 +(data[i+7]-'0')*0.1+(data[i+8]-'0')*0.01;}
                if(num!=0){Pid.kp = num;}
            }
            if((data[i] == 'I')&&(data[i+1] == '1')&&(data[i+2]=='='))
            {
                if(data[i+7]=='!'){num = (data[i+3]-'0')*1 + (data[i+5]-'0')*0.1+(data[i+6]-'0')*0.01;}
                if(data[i+8]=='!'){num = (data[i+3]-'0')*10 + (data[i+4]-'0')*1+(data[i+6]-'0')*0.1+(data[i+7]-'0')*0.01;}
                if(data[i+9]=='!'){num = (data[i+3]-'0')*100 + (data[i+4]-'0')*10 + (data[i+5]-'0')*1 +(data[i+7]-'0')*0.1+(data[i+8]-'0')*0.01;}
                if(num!=0){Pid.ki = num;}
            }
            if((data[i] == 'D')&&(data[i+1] == '1')&&(data[i+2]=='='))
            {
                if(data[i+7]=='!'){num = (data[i+3]-'0')*1 + (data[i+5]-'0')*0.1+(data[i+6]-'0')*0.01;}
                if(data[i+8]=='!'){num = (data[i+3]-'0')*10 + (data[i+4]-'0')*1+(data[i+6]-'0')*0.1+(data[i+7]-'0')*0.01;}
                if(data[i+9]=='!'){num = (data[i+3]-'0')*100 + (data[i+4]-'0')*10 + (data[i+5]-'0')*1 +(data[i+7]-'0')*0.1+(data[i+8]-'0')*0.01;}
                if(num!=0){Pid.kd = num;}
            }
            if((data[i] == 'P')&&(data[i+1] == '2')&&(data[i+2]=='='))
            {
                if(data[i+7]=='!'){num = (data[i+3]-'0')*1 + (data[i+5]-'0')*0.1+(data[i+6]-'0')*0.01;}
                if(data[i+8]=='!'){num = (data[i+3]-'0')*10 + (data[i+4]-'0')*1+(data[i+6]-'0')*0.1+(data[i+7]-'0')*0.01;}
                if(data[i+9]=='!'){num = (data[i+3]-'0')*100 + (data[i+4]-'0')*10 + (data[i+5]-'0')*1 +(data[i+7]-'0')*0.1+(data[i+8]-'0')*0.01;}
                if(num!=0){AnglePid.kp = num;}
            }
            if((data[i] == 'I')&&(data[i+1] == '2')&&(data[i+2]=='='))
            {
                if(data[i+7]=='!'){num = (data[i+3]-'0')*1 + (data[i+5]-'0')*0.1+(data[i+6]-'0')*0.01;}
                if(data[i+8]=='!'){num = (data[i+3]-'0')*10 + (data[i+4]-'0')*1+(data[i+6]-'0')*0.1+(data[i+7]-'0')*0.01;}
                if(data[i+9]=='!'){num = (data[i+3]-'0')*100 + (data[i+4]-'0')*10 + (data[i+5]-'0')*1 +(data[i+7]-'0')*0.1+(data[i+8]-'0')*0.01;}
                if(num!=0){AnglePid.ki = num;}
            }
            if((data[i] == 'D')&&(data[i+1] == '2')&&(data[i+2]=='='))
            {
                if(data[i+7]=='!'){num = (data[i+3]-'0')*1 + (data[i+5]-'0')*0.1+(data[i+6]-'0')*0.01;}
                if(data[i+8]=='!'){num = (data[i+3]-'0')*10 + (data[i+4]-'0')*1+(data[i+6]-'0')*0.1+(data[i+7]-'0')*0.01;}
                if(data[i+9]=='!'){num = (data[i+3]-'0')*100 + (data[i+4]-'0')*10 + (data[i+5]-'0')*1 +(data[i+7]-'0')*0.1+(data[i+8]-'0')*0.01;}
                if(num!=0){AnglePid.kd = num;}
            }
            if((data[i] == 'P')&&(data[i+1] == '3')&&(data[i+2]=='='))
            {
                if(data[i+7]=='!'){num = (data[i+3]-'0')*1 + (data[i+5]-'0')*0.1+(data[i+6]-'0')*0.01;}
                if(data[i+8]=='!'){num = (data[i+3]-'0')*10 + (data[i+4]-'0')*1+(data[i+6]-'0')*0.1+(data[i+7]-'0')*0.01;}
                if(data[i+9]=='!'){num = (data[i+3]-'0')*100 + (data[i+4]-'0')*10 + (data[i+5]-'0')*1 +(data[i+7]-'0')*0.1+(data[i+8]-'0')*0.01;}
                if(num!=0){RatePid.kp = num;}
            }
            if((data[i] == 'I')&&(data[i+1] == '3')&&(data[i+2]=='='))
            {
                if(data[i+7]=='!'){num = (data[i+3]-'0')*1 + (data[i+5]-'0')*0.1+(data[i+6]-'0')*0.01;}
                if(data[i+8]=='!'){num = (data[i+3]-'0')*10 + (data[i+4]-'0')*1+(data[i+6]-'0')*0.1+(data[i+7]-'0')*0.01;}
                if(data[i+9]=='!'){num = (data[i+3]-'0')*100 + (data[i+4]-'0')*10 + (data[i+5]-'0')*1 +(data[i+7]-'0')*0.1+(data[i+8]-'0')*0.01;}
                if(num!=0){RatePid.ki = num;}
            }
            if((data[i] == 'D')&&(data[i+1] == '3')&&(data[i+2]=='='))
            {
                if(data[i+7]=='!'){num = (data[i+3]-'0')*1 + (data[i+5]-'0')*0.1+(data[i+6]-'0')*0.01;}
                if(data[i+8]=='!'){num = (data[i+3]-'0')*10 + (data[i+4]-'0')*1+(data[i+6]-'0')*0.1+(data[i+7]-'0')*0.01;}
                if(data[i+9]=='!'){num = (data[i+3]-'0')*100 + (data[i+4]-'0')*10 + (data[i+5]-'0')*1 +(data[i+7]-'0')*0.1+(data[i+8]-'0')*0.01;}
                if(num!=0){RatePid.kd = num;}
            }

        }
    }

    return num;
}


uint8 date_buff[64];
uint8 date_cache[24];//数据缓存
uint8 data_flag=0;
uint8 len_sum=0;
uint16 pwm = 0;

uint32 receive_wireless(uint8* date_buff , uint8 len)
{
    uint32 num = wireless_uart_read_buffer(date_buff,len);
    if(num!=0)
    {
        //memset(date_cache, date_buff, sizeof date_buff);
        if(date_buff[0]=='P' && date_buff[1]=='1' &&date_buff[2]=='=' && date_buff[num-1]=='!')//接收数据完整
        {
            Pid.kp = Get_Gata(date_buff,num,1);
            memset(date_buff, 0, sizeof date_buff);
        }
        else if(date_buff[0]=='I' && date_buff[1]=='1' && date_buff[2]=='=' && date_buff[num-1]=='!')//接收数据完整
        {
            Pid.ki = Get_Gata(date_buff,num,1);
            memset(date_buff, 0, sizeof date_buff);
        }
        else if(date_buff[0]=='D' && date_buff[1]=='1' && date_buff[2]=='=' && date_buff[num-1]=='!')//接收数据完整
        {
            Pid.kd = Get_Gata(date_buff,num,1);
            memset(date_buff, 0, sizeof date_buff);
        }
        if(date_buff[0]=='P' && date_buff[1]=='2' &&date_buff[2]=='=' && date_buff[num-1]=='!')//接收数据完整
        {
            AnglePid.kp = Get_Gata(date_buff,num,1);
            memset(date_buff, 0, sizeof date_buff);
        }
        else if(date_buff[0]=='I' && date_buff[1]=='2' && date_buff[2]=='=' && date_buff[num-1]=='!')//接收数据完整
        {
            AnglePid.ki = Get_Gata(date_buff,num,1);
            memset(date_buff, 0, sizeof date_buff);
        }
        else if(date_buff[0]=='D' && date_buff[1]=='2' && date_buff[2]=='=' && date_buff[num-1]=='!')//接收数据完整
        {
            AnglePid.kd = Get_Gata(date_buff,num,1);
            memset(date_buff, 0, sizeof date_buff);
        }
        if(date_buff[0]=='P' && date_buff[1]=='3' &&date_buff[2]=='=' && date_buff[num-1]=='!')//接收数据完整
        {
            RatePid.kp = Get_Gata(date_buff,num,1);
            memset(date_buff, 0, sizeof date_buff);
        }
        else if(date_buff[0]=='I' && date_buff[1]=='3' && date_buff[2]=='=' && date_buff[num-1]=='!')//接收数据完整
        {
            RatePid.ki = Get_Gata(date_buff,num,1);
            memset(date_buff, 0, sizeof date_buff);
        }
        else if(date_buff[0]=='D' && date_buff[1]=='3' && date_buff[2]=='=' && date_buff[num-1]=='!')//接收数据完整
        {
            RatePid.kd = Get_Gata(date_buff,num,1);
            memset(date_buff, 0, sizeof date_buff);
        }
        /////////////////接收数据包有无，fifo循环读取/////////////////
        else
        {
           /*
            *
            *
            * */
            if((len_sum+num)>len){len_sum=0;}
            for(int i = 0;i < num;i++)
            {
                date_cache[len_sum++] = date_buff[i];
            }
            Get_Gata(date_cache,len,2);
            memset(date_buff, 0, sizeof date_buff);
        }

    }



    return num;
}








